/*
	需求：实现串口读取数据对串口读取的数据执行相应的动作
		1.接收运动指令发布控制底盘运动话题
		2.接收抓取放置指令发布控制机械臂进行话题
		3.接受自动导航指令发布控制小车进行自动导航话题
	实现流程：
		1.包含头文件
		2.初始化hm_read_node节点
		3.实例化ros句柄
		4.实例化发布者对象
		5.开启串口实现
		6.串口接收到数据实现相应动作实现
*/

// 1.包含头文件
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h> // cmd_vel
#include <string>
#include <std_msgs/Empty.h>
#include <iostream>
#include <fstream>
#include <ros/package.h>
using namespace std;

bool can_grasp = true;
bool can_release = false;

int readNumToArray(string buff, float *data)
{
	int i = 0; // 行数i
	vector<float> nums;
	// string->char *
	char *s_input = (char *)buff.c_str();
	printf("\n %s jj %s \n", s_input, buff.c_str());
	const char *split = ",";
	// 以‘,’为分隔符拆分字符串
	char *p = strtok(s_input, split);
	float a;
	printf("eee:");
	while (p != NULL)
	{
		a = atof(p);
		nums.push_back(a);
		p = strtok(NULL, split);
	}
	for (int b = 0; b < nums.size(); b++)
	{
		data[b] = nums[b];
		printf("%f ", data[b]);
	}
	printf("end size :%d\n", nums.size());
	return nums.size();
}

int main(int argc, char **argv)
{
	// 2.初始化hm_read_node节点
	ros::init(argc, argv, "hm_read_node");

	// 3.实例化ros句柄
	ros::NodeHandle nh;

	// 4.实例化发布者对象
	ros::Publisher pub1 = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);			  // 底盘控制话题
	ros::Publisher pub2 = nh.advertise<std_msgs::String>("/grasp", 1);					  // 机械臂抓取话题
	ros::Publisher pub_lp = nh.advertise<std_msgs::String>("/mark_nav", 1);				  // 学习途经点的话题
	ros::Publisher pub_start = nh.advertise<std_msgs::String>("/hm_task_match_start", 1); // 开始话题

	ros::Publisher sow_pub = nh.advertise<std_msgs::String>("/sow_topic", 1);	  // 种植话题
	ros::Publisher mode_pub = nh.advertise<std_msgs::String>("/control_mode", 1); // 流程模式话题
	// ros::Publisher pub2 = nh.advertise<std_msgs::String>("board_ip_topic", 1);

	serial::Serial _serial;
	geometry_msgs::Twist cmd;
	std_msgs::String result;
	std_msgs::String grasp_cmd;
	std_msgs::String move_goal_cmd;
	std_msgs::String start_cmd;

	std_msgs::String learn_point_cmd;

	std_msgs::String msg;

	std_msgs::String sow_cmd;
	std_msgs::String control_mode_cmd;
	// 移动速度
	float walk_vel = 0.2;
	// 旋转速度
	float yaw_rate = 0.6;
	// 参数初始化
	float speed = 0;
	float turn = 0;

	ros::Rate loop_rate(20);
	// 5.开启串口实现
	try
	{
		_serial.setPort("/dev/SPARK-HM-PI");
		_serial.setBaudrate(115200);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		_serial.setTimeout(to);
		_serial.open();
		ROS_INFO_STREAM("Port has been open successfully");
	}
	catch (serial::IOException &e)
	{
		ROS_ERROR_STREAM("Unable to open port");
		return -1;
	}

	if (_serial.isOpen())
	{
		ROS_INFO_STREAM("Port is open");
	}

	while (ros::ok()) // publish positionesian coordinates
	{
		// 6.串口接收到数据实现相应动作实现
		if (_serial.available())
		{
			result.data.clear();
			result.data = _serial.read(_serial.available());
			ROS_INFO_STREAM("Read:" << result.data);
			// 停止
			if (!strcmp(result.data.c_str(), "0"))
			{
				speed = 0;
				turn = 0;
			}
			// 前进
			else if (!strcmp(result.data.c_str(), "w"))
			{
				speed = 1;
				turn = 0;
			}
			// 后退
			else if (!strcmp(result.data.c_str(), "s"))
			{
				speed = -1;
				turn = 0;
			}
			// 左转
			else if (!strcmp(result.data.c_str(), "a"))
			{
				speed = 0;
				turn = 1;
			}
			// 右转
			else if (!strcmp(result.data.c_str(), "d"))
			{
				speed = 0;
				turn = -1;
			}
			// 接收到check，回复ready
			else if (!strcmp(result.data.c_str(), "check"))
			{
				ros::Publisher msg_pub = nh.advertise<std_msgs::String>("/sow_progress", 1);
				msg.data = "ready";
				msg_pub.publish(msg);
			}
			// 学习途经点A
			else if (!strcmp(result.data.c_str(), "11"))
			{
				learn_point_cmd.data = "learn wp_1";
				pub_lp.publish(learn_point_cmd);
				speed = 0;
				turn = 0;
			}
			// 学习途经点B
			else if (!strcmp(result.data.c_str(), "12"))
			{
				learn_point_cmd.data = "learn wp_2";
				pub_lp.publish(learn_point_cmd);
				speed = 0;
				turn = 0;
			}
			// 学习途经点C
			else if (!strcmp(result.data.c_str(), "13"))
			{
				learn_point_cmd.data = "learn wp_3";
				pub_lp.publish(learn_point_cmd);
				speed = 0;
				turn = 0;
			}
			// 学习途经点D
			else if (!strcmp(result.data.c_str(), "14"))
			{
				learn_point_cmd.data = "learn wp_4";
				pub_lp.publish(learn_point_cmd);
				speed = 0;
				turn = 0;
			}
			// 发送开始
			else if (!strcmp(result.data.c_str(), "start"))
			{
				start_cmd.data = "start";
				pub_start.publish(start_cmd);
			}
			// wifi设置
			else if (!strcmp(result.data.c_str(), "wifi_msg"))
			{
				std::fstream m_fs;
				m_fs.open("/home/spark/wifi_info.txt", ios::in);
				if (!m_fs.is_open())
				{
					cout << "读取文件失败" << endl;
				}
				string buf;
				string str_buf;
				str_buf = "set_wifi:";
				while (getline(m_fs, buf))
				{
					cout << buf << endl;
					str_buf = str_buf + buf;
				}
				printf("msg:%s\n", str_buf.c_str());
				if (!str_buf.empty())
				{
					_serial.write(str_buf);
					printf("send:%s\n", str_buf.c_str());
				}
			}
			// 模式接收并设置
			else if (!strcmp(result.data.c_str(), "auto"))
			{
				control_mode_cmd.data = "auto_mode";
				mode_pub.publish(control_mode_cmd);
			}
			else if (!strcmp(result.data.c_str(), "manual"))
			{
				control_mode_cmd.data = "manual_mode";
				mode_pub.publish(control_mode_cmd);
			}

			else if (!strcmp(result.data.c_str(), "23"))
			{
				sow_cmd.data = "sow1";
				sow_pub.publish(sow_cmd);
			}
			else if (!strcmp(result.data.c_str(), "31"))
			{
				sow_cmd.data = "sow2";
				sow_pub.publish(sow_cmd);
			}
			else
			{
				speed = 0;
				turn = 0;
			}
			cmd.linear.x = speed * walk_vel;
			cmd.angular.z = turn * yaw_rate;
			pub1.publish(cmd); // 发布/cmd_vel消息
		}
		ros::spinOnce();
		loop_rate.sleep();
	}
}
